// =============== 001.CPP  =============== //
#include <iostream>
#include "stdio.h"

using namespace std;

// ============ Leon's Kalman  ============ //

class leon_kalman {
    public:
    leon_kalman(void) {
        n = 0;
        sum = 0.0;
    }

    double calculate(double data);

    private:
    double sum;
    int n;
};

double leon_kalman::calculate(double data) {
    n++;
    sum += data;

    // for safer convert n to double before 1/n
    double rslt = sum / (double)n;
    printf("(%d)\tleon_kalman: %f\n", n, rslt);
    return rslt;
}

// ============ original Kalman  ============ //

class orgin_kalman {
    public:
    orgin_kalman(void) {
        xn = 0; // no need initial guess anymore
        n = 0;
    }

    double calculate(double data);

    private:
    double xn;
    int n;
};

double orgin_kalman::calculate(double data) {
    n++;
    // make sure convert n to double before 1/n
    // othervise, 1/n is 0
    xn = xn + (1/(double)n) * (data - xn);
    printf("d(%d)origin_kalman: %f\n", n, xn);
    return xn;
}

// ============ test code  ============ //

#define TEST_LEN 10
double test_pattern1[TEST_LEN] = {
    1 ,
    2 ,
    3 ,
    4 ,
    5 ,
    6 ,
    7 ,
    8 ,
    9 ,
    10,
};

double test_pattern2[TEST_LEN] = {
    10,
    9 ,
    8 ,
    7 ,
    6 ,
    5 ,
    4 ,
    3 ,
    2 ,
    1 ,
};

int main(void) {
	printf("%s, %s\n", __DATE__, __TIME__);
    int i;
    leon_kalman lk1, lk2;

    for(i = 0; i < TEST_LEN; i++) {
        lk1.calculate(test_pattern1[i]);
    }
    printf("\n=============\n");
    for(i = 0; i < TEST_LEN; i++) {
        lk2.calculate(test_pattern2[i]);
    }

    printf("\n=============\n");
    printf("\n=============\n");

    orgin_kalman ok1, ok2;
    for(i = 0; i < TEST_LEN; i++) {
        ok1.calculate(test_pattern1[i]);
    }
    printf("\n=============\n");
    for(i = 0; i < TEST_LEN; i++) {
        ok2.calculate(test_pattern2[i]);
    }

    return 0;
}


// =============== 002.CPP  =============== //

#include <iostream>
#include "stdio.h"

using namespace std;

// ============ Kalman alpha-beta filter for tracking  ============ //

class Kalman_alpha_beta_filter_tracking {
    public:
    Kalman_alpha_beta_filter_tracking(double init_x, double init_speed, double t) {
        x = init_x;
        speed = init_speed;
        alpha = 0.2;
        beta = 0.1;
        this->t = t;
        interation = 0;

        printf("\na Kalman_alpha_beta_filter_tracking model created\n");
        printf("x = %f, speed = %f, t = %f, alpha = %f, beta = %f\n\n", x, speed, t, alpha, beta);

        // iteration 0 will do a predict session once. no update session needed
        x = x + t * speed;
        printf("(0) predict x = %f\n", x);
    }

    void setAlphaBete(double alpha, double beta) {
        if(alpha < 0 || alpha > 1.0) {
            printf("alpha illegal.\n");
        }else{
            this->alpha = alpha;
            printf("alpha set to %f\n", alpha);
        }

        if(beta < 0 || beta > 1.0) {
            printf("beta illegal.\n");
        }else{
            this->beta = beta;
            printf("beta set to %f\n", beta);
        }
    }

    double tracking(double zn);

    private:
    double x, speed;
    double alpha, beta;
    double t;
    int interation;
};

double Kalman_alpha_beta_filter_tracking::tracking(double zn) {
    double pre_x = x;

    // update x
    x = pre_x + alpha * (zn - pre_x);
    double updated_x = x;

    // update speed
    speed = speed + beta * (zn - pre_x) / t;

    // predict x
    x = x + t * speed;

    // print internally for debug
    interation++;
    printf("(%d) z = %.0f, updated x = %f, updated speed = %f, predict x = %f\n", interation, zn, updated_x, speed, x);

    // output predict x
    return x;
}

// ============ test code  ============ //

#define TEST_LEN 10
extern double test_pattern[TEST_LEN];

int main(void) {
	printf("%s, %s\n", __DATE__, __TIME__);
    int i;

    Kalman_alpha_beta_filter_tracking demo(30000, 40, 5);
    for(i = 0; i < TEST_LEN; i++) {
        demo.tracking(test_pattern[i]);
    }

    return 0;
}

double test_pattern[TEST_LEN] = {
    30110,
    30265,
    30740,
    30750,
    31135,
    31015,
    31180,
    31610,
    31960,
    31865,
};



// =============== 003.CPP  =============== //

#include <iostream>
#include "stdio.h"

using namespace std;

// ============ Kalman alpha-beta-gama filter for tracking  ============ //

class Kalman_alpha_beta_gama_filter_tracking {
    public:
    Kalman_alpha_beta_gama_filter_tracking(double init_x, double init_v, double init_a, double t) {
        x = init_x;
        v = init_v;
        a = init_a;

        alpha = 0.5;
        beta = 0.4;
        gama = 0.1;

        this->t = t;
        interation = 0;

        printf("\na Kalman_alpha_beta_gama_filter_tracking model created\n");
        printf("x = %.2f, v = %f, a = %f, t = %f, alpha = %f, beta = %f, gama = %f\n", x, v, a, t, alpha, beta, gama);


        // iteration 0 will do a predict session once. no update session needed
        x = x + v * t + 0.5 * a * t * t;    // s = s0 + 1/2 a t^2
        v = v + a * t;                      // v = v0 + a t

        printf("(0) predict x = %f, v = %f\n", x, v);
    }

    void setAlphaBeteGama(double alpha, double beta, double gama) {
        if(alpha < 0 || alpha > 1.0) {
            printf("alpha illegal.\n");
        }else{
            this->alpha = alpha;
            printf("alpha set to %f\n", alpha);
        }

        if(beta < 0 || beta > 1.0) {
            printf("beta illegal.\n");
        }else{
            this->beta = beta;
            printf("beta set to %f\n", beta);
        }

        if(gama < 0 || gama > 1.0) {
            printf("gama illegal.\n");
        }else{
            this->gama = gama;
            printf("gama set to %f\n", gama);
        }
    }

    double tracking(double zn);

    private:
    double x, v, a;
    double alpha, beta, gama;
    double t;
    int interation;
};

double Kalman_alpha_beta_gama_filter_tracking::tracking(double zn) {
    double pre_x = x;

    // update x
    x = pre_x + alpha * (zn - pre_x);
    double updated_x = x;

    // update v
    v = v + beta * (zn - pre_x) / t;
    double updated_v = v;

    // update a
    a = a + gama * (zn - pre_x) / (0.5 * t * t);

    // predict x
    x = x + v * t + 0.5 * a * t * t; // s = s0 + 1/2 a t^2

    // predict v
    v = v + a * t;  // v = v0 + a t

    // print internally for debug
    interation++;
    printf("(%d) z = %.0f, updated x = %f, v = %f, a = %f; predict x = %f, v = %f\n", interation, zn, updated_x, updated_v, a, x, v);

    // output predict x
    return x;
}

// ============ test code  ============ //

#define TEST_LEN 10
extern double test_pattern[TEST_LEN];

int main(void) {
	printf("%s, %s\n", __DATE__, __TIME__);
    int i;

    Kalman_alpha_beta_gama_filter_tracking demo(30000, 50, 0, 5);
    for(i = 0; i < TEST_LEN; i++) {
        demo.tracking(test_pattern[i]);
    }

    return 0;
}

double test_pattern[TEST_LEN] = {
    30160,
    30365,
    30890,
    31050,
    31785,
    32215,
    33130,
    34510,
    36010,
    37265,
};


// =============== 004.CPP  =============== //

#include <iostream>
#include "stdio.h"

using namespace std;

// ============ general moving average filter for smooth measurement value  ============ //

class moving_average {
    public:
    moving_average(int n) {
        this->n = n;
        sum = 0.0;
        printf("\nA %d samples moving_average model created\n", n);
    }

    double filtering(double in);

    private:
    int n;
    double sum;
};

double moving_average::filtering(double in) {
    static int cnt = 0;

    cnt++;
    sum += in;
    double rslt = sum/cnt;

    // internal print for debug
    printf("(%d) in = %f, out = %f\n", cnt, in, rslt);

    if(cnt > n) {
        cnt = 1;
        sum = rslt;
        printf("restart...\n");
    }

    return rslt;
}

// ============ test code  ============ //

#define TEST_LEN 16
extern double test_pattern[TEST_LEN];

int main(void) {
    printf("%s, %s\n", __DATE__, __TIME__);
    int i;

    moving_average demo(1000); // much longer than actual len means "never restart..."
    for(i = 0; i < TEST_LEN; i++) {
        demo.filtering(test_pattern[i]);
    }

    return 0;
}

double test_pattern[TEST_LEN] = {
    51, // 0
    48, // 1
    47, // 2
    52, // 3
    51, // 4
    48, // 5
    49, // 6
    53, // 7
    48, // 8
    49, // 9
    52, // 10
    53, // 11
    51, // 12
    52, // 13
    49, // 14
    50, // 15
};

